#include <iostream>
#include <iomanip>
#include <fstream>

#include "Mobile__test.h"
#include "..\Mobile.h"
#include <cfloat>

#define REPEAT(N) for (long long i = 0; i < (N); i++)

static void SetMobileState(Mobile *m)
{
	m->SetPosition(0.278498218867048f, 0.546881519204984f, 0.957506835434298f);
	m->SetLinearImpulse(0.957166948242946f, -0.485375648722841f, 0.8002804688888f);
	m->SetMass(1.26986816293506f);

	Triple *f = new Triple();
	f->Set(-0.905791937075619f, 0.513375856139019f, 0.63235924622541f);

	Triple *r = new Triple(); // doesn't matter
	r->Reset();

	m->AddForce(*f, *r);
}

void Mobile__speed_test()
{
	ofstream file("Mobile__test.m");

#ifdef SSSE3
	file << "% SSSE3 code" << endl;
#else
	file << "% standard code" << endl;
#endif

	Mobile *m = new Mobile();
	SetMobileState(m);

	const FP_DATATYPE dt = 0.1f;

	const unsigned long long ts = rdtsc();
#define E 28
	REPEAT(1LL << E)
	{
		m->Translate(dt);
	}
	unsigned long long exec_time = (unsigned long long)(rdtsc() - ts) >> E;

	file << exec_time << endl;

	file.close();
}

void Mobile__correctness_translation_test()
{
	Mobile *m = new Mobile();
	SetMobileState(m);

	const FP_DATATYPE dt = 0.0975404f;

	// record dt
	ofstream file_dt("..\\..\\matlab\\dt.txt");
	file_dt << setprecision(16) << dt << endl;
	file_dt.close();

	unsigned int c = _control87(_CW_DEFAULT | _RC_NEAR, _MCW_RC); // Configure FPU rounding mode

	const unsigned int n = 5000;

	// move mobile
	// record mobile position -- this is used to observe the rounding mode effects
	ofstream file_pos("..\\..\\matlab\\Mobile_position.txt");
	for (int i = 0; i < n; i++)
	{
		m->Translate(dt);

		file_pos << m->GetPosition() << endl;
	}
	file_pos.close();

	Triple& final_pos = m->GetPosition();
	const float matlab_final_pos[3] = {-3322.789186315427f, 1888.349858316182f, 2433.684127720488f}; // !! outdated

	// permitted error
	const double epsilon = 0.01f;

	// verification
	bool flag = true;
	for (int i = 0; i < 3; i++)
	{
		const FP_DATATYPE error = matlab_final_pos[i] - final_pos[i];
		if (fabs(error) > epsilon)
		{
			flag = false;
			cerr << "Mobile correctness test fail (error = " << error << ")" << endl;
		}
	}

	if (flag)
	{
		cout << "Mobile correctness test pass" << endl;
	}
}

void Mobile__correctness_rotation_test()
{
	Mobile *m = new Mobile();
	m->SetRotation(1.0f, 0.0f, 0.0f, 0.0f);
	m->SetAngularImpulse(0.716956948242946f, -0.648748537522841f, 0.0480028688888f);
	m->SetDefaultInertia();

	m->SetTorque(0.015f, 0.667f, -0.333f);

	const FP_DATATYPE dt = 0.01f;//0.0975404f;

	// record dt
	ofstream file_dt("..\\..\\matlab\\dt.txt");
	file_dt << setprecision(16) << dt << endl;
	file_dt.close();

	unsigned int c = _control87(_CW_DEFAULT | _RC_NEAR, _MCW_RC); // Configure FPU rounding mode

	const unsigned int n = 5000;

	// rotate mobile
	// record mobile rotation -- this is used to observe the rounding mode effects
	ofstream file_rot("..\\..\\matlab\\Mobile_rotation.txt");
	file_rot << setprecision(9);
	for (int i = 0; i < n; i++)
	{
		m->Move__rot_test(dt);

		file_rot << m->GetRotation() << endl;
	}
	file_rot.close();

	Quaternion& final_q = m->GetRotation();
	const float matlab_final_q[3] = {-3322.789186315427f, 1888.349858316182f, 2433.684127720488f}; // !! outdated

	// permitted error
	const double epsilon = 0.01f;

	// verification
	bool flag = true;
	for (int i = 0; i < 3; i++)
	{
		const FP_DATATYPE error = matlab_final_q[i] -final_q[i];
		if (fabs(error) > epsilon)
		{
			flag = false;
			cerr << "Mobile correctness test fail (error = " << error << ")" << endl;
		}
	}

	if (flag)
	{
		cout << "Mobile correctness test pass" << endl;
	}
}
